package naito_rescue.router;

import rescuecore2.misc.*;
import rescuecore2.misc.geometry.*;
import rescuecore2.worldmodel.*;
import rescuecore2.standard.entities.*;

import naito_rescue.*;
import naito_rescue.agent.*;
import naito_rescue.router.object.*;
import naito_rescue.object.*;
import java.util.*;
import java.util.Map.*;

public class AdaptiveAStarModule extends AbstractRoutingModule
{
	protected EntityID goal = null;
	protected int counter = 0;

	//----------------
	// Variables for Adaptive A*
	//----------------
	
	/* 保存されたCLOSEDリスト */
	/*   . Map<目的地ノードのID, CLOSED-LIST> の形式で保存する */
	Map<EntityID, Map<EntityID, AdaptiveSearchNode>> savedClosedList = null;
	public AdaptiveAStarModule(NAITOAgent owner, ConnectivityGraph graph){
		super(owner, graph);
		
		this.savedClosedList = new HashMap<EntityID, Map<EntityID, AdaptiveSearchNode>>();
	}
	
	public List<EntityID> search(EntityID start, EntityID goal){
	
		//----------------
		// Variables for A*
		//----------------
		PriorityQueue<AdaptiveSearchNode> openList = new PriorityQueue<AdaptiveSearchNode>();
		Map<EntityID, AdaptiveSearchNode> closedList = new HashMap<EntityID, AdaptiveSearchNode>();
		
		//----------------
		// Variables for AdaptiveA*
		//----------------
		boolean isReplanning = false;
		//----------------
		// Variables for logging.
		//----------------
		int prunedCount = 0;
		int expandedCount = 0;
		long startTime, stopTime;
		String context = "AdaptiveAStar#backSearch";
		
		//----------------
		// Process start.
		//----------------
		startTime = System.currentTimeMillis();
		
		//----------------
		// 保存されたCLOSEDリストの取得
		//   . 現在の目的地ノード( = 引数goal)に対して保存されたCLOSEDリストがあれば，
		//     今回の探索に使用する
		//   . そうでなければ，新しい探索として探索エピソードの初期化を行なう
		//----------------
		//logger.info("----", context);
		if(this.savedClosedList.get(goal) == null){
			/* 探索エピソードの初期化 */
			//logger.info("The first path-planning to " + goal, context);
			counter = 1;
		}else{
			counter++;
			//logger.info("Re-planning to " + goal + " (" + counter + " th planning)", context);
			isReplanning = true;
		}
		//----------------
		// 初期ノードの登録 
		//   . AdaptiveSearchNode#<Constructor>の引数
		//     * NodeID, GoalID, Parent, distanceToParent, heuristicValue, counter
		//     * BackSearchなので，現在着目しているゴールIDが出発地点で，
		//       経路探索を開始するスタートIDが目的地のIDとなることに注意する
		//----------------
		int initHeuristicValue = model.getDistance(goal, start);
		openList.add(new AdaptiveSearchNode(goal, start, null, 0, initHeuristicValue, counter, logger));
		
		AdaptiveSearchNode currentNode = null;
		boolean searchComplete = false;
		Set<EntityID> neighbors = null;
		
		/* 探索ループ処理 */
		while (! openList.isEmpty() && ! searchComplete)
		{
			//logger.debug("\n----------------", context);
			//logger.debug("OPEN=" + openList, context);
			/* OPENリストの中から，コスト推定値が最小となるノードを取り出す */
			currentNode = openList.remove();
			//logger.debug("currentNode = " + currentNode, context);
			if (currentNode.getNodeID().equals(start))
			{
				/* 探索がスタートノードまで至ったら探索終了 */
				searchComplete = true;
				break;
			}
			
			//logger.debug("currentNode = " + currentNode, context);
			//
			if (closedList.containsKey(currentNode.getNodeID()))
			{
				//logger.debug("closedList contains " + currentNode.getNodeID(), context+" if(contains)");
				// check distance of the previous path found to this node
				AdaptiveSearchNode previousNode = closedList.get(currentNode.getNodeID());
				
				//logger.debug("(" + currentNode.getNodeID() + ") previousNode path length = " + previousNode.getPathLength(), context+" if(contains)");
				//logger.debug("(" + currentNode.getNodeID() + ") currentNode  path length = " + currentNode.getPathLength(), context+" if(contains)");
				if (previousNode.getPathLength() > currentNode.getPathLength())
				{
					prunedCount++;
					continue;
				}
			}
			
			/* closedListにcurrentNodeを登録する */
			expandedCount++;
			closedList.put(currentNode.getNodeID(), currentNode);
			
			/* currentNodeに隣接するノードを展開する */
			neighbors = graph.getNeighbors(currentNode.getNodeID());
			
			//logger.debug("neighbors = " + neighbors, context);
			for (EntityID id: neighbors)
			{
				if (! closedList.containsKey(id))
				{
					if(checker.isPassableTo(currentNode.getNodeID(), id)){
						/* currentNodeから，展開したノード"id"までのコストを計算する */
						int distanceToCurrentNode = model.getDistance(id, currentNode.getNodeID());
						/* 展開したノード"id"から，スタートノードまでのコストを推定する */
						int heuristicValue = model.getDistance(id, start);
						if(isReplanning){
							//logger.debug("Re-planning to " + goal + ", calculate first heuristic. node = " + id + ", h = " + heuristicValue, context);
						}
						
						//----------------
						// ヒューリスティックの更新(再探索の場合)
						//----------------
						if(isReplanning){
							Map<EntityID, AdaptiveSearchNode> closedListToCurrentGoal = this.savedClosedList.get(goal);
							AdaptiveSearchNode node = closedListToCurrentGoal.get(id);
							if(node != null){
								//logger.debug("Use updated heuristic. Node = " + node, context);
								heuristicValue = node.getUpdatedHeuristic();
								//logger.debug("Re-planning to " + goal + ", use updated heuristic. node = " + id + ", h = " + heuristicValue,context);
							}
						}
						/* openListに，展開したノードを追加する */
						openList.add(new AdaptiveSearchNode(id, goal, currentNode, distanceToCurrentNode, heuristicValue, counter, logger));
					}
				}
			}
		}
		if (! searchComplete)
		{
			// no path found 
			//logger.info("No path found to " + goal.getValue() + ".",context);
			//logger.info("----", context);
			return null;
		}
		/* 経路探索の終了 -> 経路の構築 */
		List<EntityID> path = new LinkedList<EntityID>();
		while (currentNode.getParent() != null)
		{
			path.add(currentNode.getNodeID());
			currentNode = currentNode.getParent();
		}
        path.add(currentNode.getNodeID());
        path.remove(0);
		int totalCost = computePathLength(path);

		//----------------
		// CLOSEDリストの保存，
		// ヒューリスティックの更新
		//----------------
		//logger.info("Update node's heuristic in CLOSED list.");
		for(Entry<EntityID, AdaptiveSearchNode> entry : closedList.entrySet()){
			AdaptiveSearchNode node = entry.getValue();
			node.updateHeuristicValue(totalCost);
		}
		this.savedClosedList.put(goal, closedList);		
		
		
		stopTime = System.currentTimeMillis();
		long processTime = stopTime - startTime;
		//logger.info("Find path: " + ", Total cost = " + totalCost + ", expandedCount = " + expandedCount + ", processTime = " + processTime + "(ms)" + ", pruned count = " + prunedCount, context);
		//logger.info("Path = " + path, context);
		//logger.info("----", context);
		return path;
	}
}
